//
// Created by chizuru on 2025/9/29.
//

#include "SensorTPController.h"

SensorTPController::SensorTPController(SensorTPDevice &s, std::shared_ptr<TinyXmlRegisterTPMap> map, RetryPolicy rp)
    : device(s)
    , regMap(map)
    , rp(rp) {

}

OperationResult
SensorTPController::getRegisters(uint16_t dev, const std::vector<std::pair<std::string, std::string>> &regs,
                                 std::vector<std::optional<uint32_t>> &out) {
    if (regs.empty()) {
        return OperationResult::Fail(ErrorCode::InvalidAddress, "Empty register list");
    }

    auto firstAddrOpt = regMap->getAddress(regs[0].first, regs[0].second);
    std::vector<uint16_t> registerCounts;
    for (auto&& reg: regs) {
        auto numOpt = regMap->getNumOfReg(reg.first, reg.second);
        if (!numOpt) {
            std::string eStr = "Unknown reg " + get<0>(reg) + "." + get<1>(reg) + "\n";
            return OperationResult::Fail(ErrorCode::InvalidAddress, eStr);
        }
        registerCounts.push_back(*numOpt);
    }
    auto values = device.readRegisters(dev, *firstAddrOpt, registerCounts);
    if (values.empty()) {
        return OperationResult::Fail(ErrorCode::ReadFailure, "Read register failed");
    }

    out = values;

    return OperationResult::Ok();
}

OperationResult SensorTPController::setRegisters(uint16_t dev,
                                                 const std::vector<std::tuple<std::string, std::string, uint32_t>> &regs) {
    if (regs.empty()) {
        return OperationResult::Fail(ErrorCode::InvalidAddress, "Empty register list");
    }
    auto firstAddrOpt = regMap->getAddress(get<0>(regs[0]), get<1>(regs[0]));

    std::vector<uint16_t> values;
    uint16_t registerCount;
    for (auto&& reg: regs) {
        auto numOpt = regMap->getNumOfReg(get<0>(reg), get<1>(reg));
        if (!numOpt) {
            std::string eStr = "Unknown reg " + get<0>(reg) + "." + get<1>(reg) + "\n";
            return OperationResult::Fail(ErrorCode::InvalidAddress, eStr);
        }
        registerCount += *numOpt;
        if (*numOpt > 1) {
            values.push_back(static_cast<uint16_t>(get<2>(reg) >> 16));
        }
        values.push_back(static_cast<uint16_t>(get<2>(reg) & 0x0000ffff));
    }
    auto ok = device.writeRegisters(dev, *firstAddrOpt, registerCount, values);
    if (!ok) {
        return OperationResult::Fail(ErrorCode::WriteFailure, "Write register failed");
    }
    return OperationResult::Ok();
}
